#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "roboard_drivers/adc_val.h"
#include "std_msgs/String.h"
#include <stdlib.h>
#include "roboard/roboard.h"

using namespace roboard_drivers;

int adc_interval = 200;
bool sleep_stop = false;

void change_freq(const std_msgs::Int32::ConstPtr& msg)
{
	ROS_INFO("adc(roboard):Request to change adc interval to %i", int(msg->data));
	adc_interval = int(msg->data);
	sleep_stop = true;
}

int main(int argc, char **argv)
{

    roboio_SetRBVer(RB_100);
    //roboio_SetRBVer(RB_100RD);  // if your RoBoard is RB-100RD
    //roboio_SetRBVer(RB_110);    // if your RoBoard is RB-110
    //roboio_SetRBVer(RB_050);    // if your RoBoard is RB-050

    if (spi_Init(SPICLK_21400KHZ) == false)  // init SPI lib for accessing the A/D converter
    {
        ROS_INFO("adc(roboard):ADC ERROR: fail to init SPI lib (%s)!\n", roboio_GetErrMsg());
        return -1;
    }


	if(!adc_InitMCH(ADC_USECHANNEL0+ADC_USECHANNEL1+ADC_USECHANNEL2+ADC_USECHANNEL3+ADC_USECHANNEL4+ADC_USECHANNEL5+ADC_USECHANNEL6+ADC_USECHANNEL7, ADCMODE_RANGE_2VREF,ADCMODE_UNSIGNEDCODING))
	{
		ROS_INFO("adc(roboard):ADC ERROR: fail to init MCH(%s)!\n", roboio_GetErrMsg());
		return -1;
	}
	
	int * adc_data;

	ros::init(argc, argv, "roboard");
	ros::NodeHandle n;
	ros::Publisher adc_pub	= n.advertise<adc_val>("roboard/adc", 10);
	ros::Subscriber adc_sub	= n.subscribe("roboard/adc_interval", 10, change_freq);
	//ros::spin();
	
	ros::Rate loop_rate(1000);
	struct timeval start, end;
	while (ros::ok())
	{
		gettimeofday(&start, NULL);
		adc_data=adc_ReadMCH();
		//printf("%i %i %i %i %i %i %i %i\n",adc_data[0],adc_data[1],adc_data[2],adc_data[3],adc_data[4],adc_data[5],adc_data[6],adc_data[7]);
		adc_val raw_data;
		raw_data.timestamp = start.tv_sec+double(start.tv_usec)/1000000.0;
		raw_data.val0 = adc_data[0];//adc_ReadCH(0);//
		raw_data.val1 = adc_data[1];//adc_ReadCH(1);//adc_data[1];
		raw_data.val2 = adc_data[2];//adc_ReadCH(2);//adc_data[2];
		raw_data.val3 = adc_data[3];//adc_ReadCH(3);//adc_data[3];
		raw_data.val4 = adc_data[4];//adc_ReadCH(4);//adc_data[4];
		raw_data.val5 = adc_data[5];//adc_ReadCH(5);//adc_data[5];
		raw_data.val6 = adc_data[6];//adc_ReadCH(6);//adc_data[6];
		raw_data.val7 = adc_data[7];//adc_ReadCH(7);//adc_data[7];
		adc_pub.publish(raw_data);

		ros::spinOnce();
		gettimeofday(&end, NULL);
		sleep_stop = false;
		while(!sleep_stop && (end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec)) < adc_interval*1000){
			loop_rate.sleep();
			gettimeofday(&end, NULL);
		}
		sleep_stop = false;
		ros::spinOnce();
	}
	adc_CloseMCH();
	return 0;
}
